##coding=utf-8
#c:\Python27\python dd.py
import time,os,copy,sys,plc,serial,binascii
sys.path.insert(0, ".")
import threading
#from time import sleep,time
#import pywin.debugger as dbg

import collections

import logging
from logging import handlers
import config

def get_logger(logger_name, log_file, level=logging.INFO):
    formatter = logging.Formatter('%(asctime)s : %(message)s', "%Y-%m-%d %H:%M:%S")
    fileHandler = handlers.RotatingFileHandler(log_file,'w',20000,10)
    fileHandler.setFormatter(formatter)

    vlog = logging.getLogger()#logger_name)
    vlog.setLevel(level)
    vlog.addHandler(fileHandler)

    return vlog

if 0:
    logger = logging.getLogger()
else:
    logger = get_logger('logger1','dd.log')
    logger.setLevel(logging.INFO)

AGV_COUNT = 0
AGV_TSK_CFG = config.Settings["STP_CFG4"]#AGV task configs for SDO to download 10 AGVs u16[4] stop two pos
AGV_FLIP_CFG = config.Settings["FLIP_CFG32"]
AGV_STATUS = collections.OrderedDict()
#AGV_STATUS  = {1:{'CurNode':0,'NextNode':0,'SegProgess':0.1,'StopOrRun':0,'ManuOrAuto':0,'StopNodeOrSeg':0,'InOrOutLine':0,'NoBlockOrBlock':0,'OldTskOrCfgRefreshed':0,'SysNormalOrNot':0}}#AGV Status infomation for upload PDO to Refresh
ELEVATOR_STATUS = [1,0,1]##Elevator status for PDO to download

SDOTYPE,PDOTYPE="a","z"
TIMEOUT = 10 #20秒
CYCLE_TIME = 1

from BitClass import bitwise_operator

#根据四个byte数据更新AGV状态
def AGV_status_update(id,info):
    global AGV_COUNT
    byte1,byte2 = map(lambda x:int(x,16),info)#,byte3,byte4
    #dbg.set_trace()
    if id not in AGV_STATUS.keys() and bitwise_operator(byte2,5).RX():##新增的AGV已经收到任务配置SDO
        AGV_STATUS[id]=collections.OrderedDict()
        #AGV_COUNT += 1
    AGV_STATUS[id]['CurNode_10'] = byte1
    #AGV_STATUS[id]['NextNode'] = byte2
    AGV_STATUS[id]['Info'] = byte2
    AGV_STATUS[id]['StopOrRun'] = bitwise_operator(byte2,0).RX()
    AGV_STATUS[id]['ManuOrAuto'] = bitwise_operator(byte2,1).RX()
    AGV_STATUS[id]['StopNodeOrSeg'] = bitwise_operator(byte2,2).RX()
    AGV_STATUS[id]['NotYaw'] = bitwise_operator(byte2,3).RX()
    AGV_STATUS[id]['NoBlockOrBlock'] = bitwise_operator(byte2,4).RX()
    AGV_STATUS[id]['CFGOK'] = bitwise_operator(byte2,5).RX()
    AGV_STATUS[id]['SysNormalOrAlarm'] = bitwise_operator(byte2,7).RX()
    AGV_STATUS[id]['LastTime'] = time.time()

    logger.debug("AGV %d Updated,%s"%(id,str(AGV_STATUS[id])))

def AGVStatusDictToByte4(id):
    byte2 = AGV_STATUS[id]['StopOrRun']+(AGV_STATUS[id]['ManuOrAuto']<<1)+(AGV_STATUS[id]['StopNodeOrSeg']<<2)\
            +(AGV_STATUS[id]['NotYaw']<<3)+(AGV_STATUS[id]['NoBlockOrBlock']<<4)+(AGV_STATUS[id]['CFGOK']<<5)\
            +(AGV_STATUS[id]['SysNormalOrAlarm']<<7)
    return byte2
def AGVStatusDictToBytes(id):
    byte1 = AGV_STATUS[id]['CurNode_10']
    byte2 = AGVStatusDictToByte4(AGV_STATUS[id])
    return [byte1,byte2]


SerialPortLock = threading.Lock() #

agv_list = []
ser = serial.Serial(port="/dev/ttyUSB0",stopbits = 1, bytesize = 8, parity = 'N', baudrate= 9600, timeout= 1)
def LoraSendSDO(node):
    ##use log to minic real serial
    cfg = config.Settings["STP_CFG4"]+config.Settings["FLIP_CFG32"]
    #cmd = "%s%d,%d,%d,%d,%d\n"%(SDOTYPE,node,cfg[0],cfg[1],cfg[2],cfg[3])
    cmd = "%s%d"%(SDOTYPE,node)
    for c in cfg:
        cmd += ",%d"%c
    crc = binascii.crc32(cmd) & 0xffffffff
    cmd += ",%x\n"%crc
    logger.info("Send SDO:%s"%cmd)
    SerialPortLock.acquire()
    if (ser.isOpen()):
        ser.write(str(cmd))
    SerialPortLock.release()
    #agv_list[node-1].cmd.set_value(str(cmd))
def LoraSendRecfg():#reconfig all
    global SSState
    cmd = "RECFG"
    logger.info("---------------------")
    SerialPortLock.acquire()
    if (ser.isOpen()):
        ser.write(str(cmd))
    SerialPortLock.release()

def LoraSendPDO(node,info):
    cmd = "%s%d,%d,%d,%d\n"%(PDOTYPE,node,info[0],info[1],info[2])
    logger.info("Send PDO:%s"%cmd)
    SerialPortLock.acquire()
    if (ser.isOpen()):
        ser.write(str(cmd)+'\n')
    SerialPortLock.release()
    #agv_list[node-1].cmd.set_value(str(cmd))
def LoraReadCmd():
    try:
        #SerialPortLock.acquire()
        c = ser.readline()
        #SerialPortLock.release()
        n = int(c[1])
        #logger.debug("c[1]:%d"%n)
        if c[0] == SDOTYPE or c[0] == PDOTYPE:
            logger.debug("Read Msg from %s"%c)
            return c
    except Exception,e:
        logger.debug("Exception:%s"%str(e))
    #return raw_input('input AGV msg: ')
class MsgThread(threading.Thread):
    '''
    Message Receive Thread: receive a cmd and feedback to AGV, 
    ex. Receive a1 Send AGV_TSK_CFG, Receive z1 Record 1 Pos
    '''
    def __init__(self,name):
        threading.Thread.__init__(self)
        self.sdomsg = [-1]
        self.running=1
        self.tmlastok = time.time()
    def check_heartBeat(self,time_out = 10):
        if time.time()- self.tmlastok >time_out:
            #LoraSendSDO(0,AGV_TSK_CFG)##send something to keep alive
            self.tmlastok = time.time()
    def ParseCmd(self,cmd):
        logger.debug("parse:%s"%cmd)
        try:
            sp = cmd[0]#Sdo or Pdo
            if sp == SDOTYPE:
                node = int(cmd[1])  
                self.sdomsg = node
            elif sp == PDOTYPE:#z1,1,1,1,34(0x22)
                node = int(cmd[1])
                cl = cmd.split(',')[1:]
                info = list(map(lambda x:hex(int(x)),cl))
                info.insert(0,node)
                self.pdomsg = info
                logger.debug("pdomsg:%s"%info)
        except:
            #self.running=0
            sp=''
        return sp
    def ProcessSDO(self):
        '''#mimic sdo'''
        n = self.sdomsg
        logger.info("Receive SDO:node%d"%(n))
        LoraSendSDO(n)
    def ProcessPDO(self):
        '''Receive PDO from AGV refresh AGV_STATUS'''
        n = self.pdomsg[0]
        info = self.pdomsg[1:]
        #dbg.set_trace()
        logger.info("Receive PDO:node%d,%s"%(n,str(info)))
        try:
            AGV_status_update(n,info)
        except:
            pass
    def run(self):
        while self.running:
          self.check_heartBeat()
          r = LoraReadCmd()
          if not r :
                #time.sleep(0.5)
                continue
          sp = self.ParseCmd(r)
          if sp ==PDOTYPE:
            self.ProcessPDO()
          else:
            self.ProcessSDO()
          self.tmlastok = time.time()
    def getResult(self):
        return self.running

#ADDREStop
ADDRModes,ADDRCMDCFG,ADDRCMDSDO,ADDRCMDPDO,ADDRAGVID,ADDRCFG,ADDRRMH,ADDRMode0,ADDRMode1,ADDRReset = 27,6,7,8,10,12,0,1,2,4
ADDRSTPCFG,ADDRFLIPCFG,ADDRAGVCounts,ADDRAGVPos1,ADDRAGVSTATUS1, = 12,90,30,31,61
TSK_CFG_DIRTY = 1
class ScheduleThread(threading.Thread):
    '''
    Scheduler: Receive CMD from plc then send to AGVs in download pdo
    '''
    def __init__(self,name):
        threading.Thread.__init__(self)
        self.running=1
        self.Last_Time_PDO = 0
        #plc.plcstart()
    def ClearCmd(self):
        for a in [ADDRCMDCFG,ADDRCMDSDO,ADDRCMDPDO]:
            plc.set_mbuf(1,a,0)
    def SaveConfig(self):
        #global AGV_TSK_CFG
        atc = [plc.get_mbuf(1,ADDRCFG),plc.get_mbuf(1,ADDRCFG+1),plc.get_mbuf(1,ADDRCFG+2),plc.get_mbuf(1,ADDRCFG+3)]
        config.Settings["STP_CFG4"] = atc
        for i in range(4):
            config.Settings["FLIP_CFG32"][i] = plc.get_mbuf(1,ADDRFLIPCFG+i)
        logger.info("CFG New:%x,%x,%x,%x,%d,%d"%(atc[0],atc[1],atc[2],atc[3],config.Settings["FLIP_CFG32"][0],config.Settings["FLIP_CFG32"][1]))
        f=open("config.py","w")
        f.write("Settings = %s"%str(config.Settings))
        f.close()
    def run(self):
        global TSK_CFG_DIRTY,AGV_STATUS
        while self.running:
          if plc.get_rbuf(ADDRModes) == 1:##Idle
            if plc.get_mbuf(1,ADDRCMDCFG) == 1:#CMDCFG
                self.SaveConfig()
                TSK_CFG_DIRTY = 1
                self.ClearCmd()
            elif plc.get_mbuf(1,ADDRCMDSDO) == 1:#CMDSDO
                agvid = plc.get_mbuf(1,ADDRAGVID)
                if agvid == 0:#Config All
                    print "sendsdotoall",[plc.get_mbuf(1,ADDRCFG),plc.get_mbuf(1,ADDRCFG+1),plc.get_mbuf(1,ADDRCFG+2),plc.get_mbuf(1,ADDRCFG+3)]
                else:
                    print "sendsdo",agvid,[plc.get_mbuf(1,ADDRCFG),plc.get_mbuf(1,ADDRCFG+1),plc.get_mbuf(1,ADDRCFG+2),plc.get_mbuf(1,ADDRCFG+3)]
                    LoraSendSDO(agvid)
                self.ClearCmd()
            elif plc.get_mbuf(1,ADDRCMDPDO) == 1:#CMDPDO
                print "PDO"
                self.ClearCmd()
          elif plc.get_rbuf(ADDRModes) == 2:##Normal
           self.ClearCmd()
           TT = time.time()
           if TSK_CFG_DIRTY:
                LoraSendRecfg()#reconfig all
                AGV_STATUS = collections.OrderedDict()
                TSK_CFG_DIRTY = 0
           for n in AGV_STATUS.keys():
            if TT -AGV_STATUS[n]["LastTime"]>TIMEOUT:
                AGV_STATUS[n]["CFGOK"] = 0
                del AGV_STATUS[n]
                logger.info("AGV OffLine Need Reset:%d"%n)
            #if AGV_STATUS[n]["CFGOK"] ==1 and AGV_STATUS[n]["SysNormalOrAlarm"] == 0 and AGV_STATUS[n]["ManuOrAuto"] == 1:##AGV处于自动状态没有报警才需要接收调度指令       
            #LoraSendPDO(0,ELEVATOR_STATUS)#send to all agv
           if 1:#len(AGV_STATUS)>0:#TT - self.Last_Time_PDO > CYCLE_TIME:
                LoraSendPDO(0,ELEVATOR_STATUS)#send to all agv
                self.Last_Time_PDO = TT
          plc.set_mbuf(1,ADDRAGVCounts,len(AGV_STATUS))
          for id in AGV_STATUS.keys():
            plc.set_mbuf(1,ADDRAGVPos1+id-1,AGV_STATUS[id]["CurNode_10"])
            plc.set_mbuf(1,ADDRAGVSTATUS1+id-1,AGV_STATUS[id]["Info"])
            logger.debug("AGV %d,pos:%d"%(id,plc.get_mbuf(1,ADDRAGVPos1+id-1)))
          #print len(AGV_STATUS)
          time.sleep(4)

lastn = 111

plc.plcstart()
time.sleep(0.1)

#cntaddr = 30
initSSaddr,initmapcfg1,initmapcfg2 = 1,2,3

plc.set_mbuf(1,ADDRRMH,1)#\ref gr0 FB in Idle for giwRMH
plc.set_mbuf(1,ADDRMode1,1)#\ref Idle for gixMode0
plc.set_mbuf(1,ADDRReset,1)#\ref Idle for gixReset
#plc.set_rbuf(cntaddr,lastn)#\ref rcnts FB in Init Program
plc.set_rbuf(initmapcfg1,1.0)#\ref riss FB in Init Program
plc.set_rbuf(initmapcfg2,2.0)#\ref riss FB in Init Program
plc.set_rbuf(initSSaddr,1.0)#\ref riss FB in Init Program
for i in range(4):
    plc.set_mbuf(1,ADDRSTPCFG+i,config.Settings["STP_CFG4"][i])
for i in range(4):
    plc.set_mbuf(1,ADDRFLIPCFG+i,config.Settings["FLIP_CFG32"][i])


def get_time():
    return time.time()

if __name__ == '__main__':
    m,s = [MsgThread('m'),ScheduleThread('s')]
    splt = "----"*10
    print(splt)
    logger.info(splt)
    for t in (m,s):
        t.setDaemon(True)
        t.start()
    while 1:
        time.sleep(1)
        
